#include "ros/ros.h"
#include "visualization_msgs/MarkerArray.h"
#include "visualization_msgs/Marker.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "biped_robin_msgs/StepTarget3DService.h"
#include "std_srvs/Empty.h"
#include "std_msgs/UInt8.h"
#include <math.h>

/*
Dieser Knoten empfängt eine Geschwindigkeitsvorgabe von bipedRobin_teleop/biped_joy_node, und erzeugt daraus Schritte.
Schritt werden nur gesendet wenn der Schrittbuffer im Roboter leer ist.
Die Anzahl der Schritt im Buffer wird vom bipedRobin_driver/biped_deserializer empfangen
Die auszuführen Schritte werden an bipedRobin_driver/biped_serializer gesendet
*/

#define DEF_LEG_DISTANCE 0.25
#define MIN_LEG_DISTANCE 0.22
#define MAX_LEG_DISTANCE 0.3

#define MAX_STEP_LENGTH 0.1
#define MAX_STEP_ANGLE 3.1415/8

ros::ServiceClient footstepInc_client; //sends Steps to the serializer
ros::Publisher goalVizPub;

geometry_msgs::PoseStamped goalViz;

int currentLegToMove = 1; //0.. right leg, 1.. left leg
bool sendStep = true; //Flag if we shoud send a step at time sendStepTime
int wasWalking = false; //flag that says if the robot was previously walking to know when to make the last step

ros::Time lastCallbackTime;
ros::Time thisCallbackTime;
ros::Time sendStepTime;

//TODO: use TF to get current position of the robot
tf::Transform desiredCenterPoint (tf::createQuaternionFromYaw(0), tf::Point(0, 0.271/2, 0));
tf::Transform desiredLeftLeg (tf::createQuaternionFromYaw(0), tf::Point(0, 0.271, 0));
tf::Transform desiredRightLeg (tf::createQuaternionFromYaw(0), tf::Point(0, 0, 0));

template <typename T> int sgn(T val) {
    return (T(0) < val) - (val < T(0));
}

void visualize() {
	goalViz.header.seq++;
	goalViz.header.stamp = thisCallbackTime;
	goalViz.header.frame_id = "odom";

	goalViz.pose.position.x = desiredCenterPoint.getOrigin().x(); //desiredCenterPoint
	goalViz.pose.position.y = desiredCenterPoint.getOrigin().y();
	goalViz.pose.position.z = desiredCenterPoint.getOrigin().z();

	goalViz.pose.orientation.x = desiredCenterPoint.getRotation().getX();
	goalViz.pose.orientation.y = desiredCenterPoint.getRotation().getY();
	goalViz.pose.orientation.z = desiredCenterPoint.getRotation().getZ();
	goalViz.pose.orientation.w = desiredCenterPoint.getRotation().getW();

	goalVizPub.publish(goalViz);
}


void twistCallback(const geometry_msgs::Twist twist) {
	biped_robin_msgs::StepTarget3DService srv;

	thisCallbackTime = ros::Time::now();

	// update desired center point
	double deltaTime = 0.01;
	tf::Pose deltaCenterPoint (tf::createQuaternionFromYaw(twist.angular.z * deltaTime), tf::Point(twist.linear.x * deltaTime, twist.linear.y * deltaTime, 0.0));
	desiredCenterPoint = desiredCenterPoint * deltaCenterPoint;

	/*// TODO: saturate desired Center Point
	tf::Transform deltaStanceCenter;
	if (currentLegToMove == 0) { //right leg
		deltaStanceCenter = desiredCenterPoint.inverse() * desiredLeftLeg;
	} else { //left leg
		deltaStanceCenter = desiredCenterPoint.inverse() * desiredRightLeg;
	}
	double deltaX = deltaStanceCenter.getOrigin().getX();
	double deltaY = deltaStanceCenter.getOrigin().getY();
	double deltaG = tf::getYaw(deltaStanceCenter.getRotation());
	ROS_INFO("%f %f %f",deltaX,deltaY,deltaG);*/



	if (sendStep && (thisCallbackTime - sendStepTime).toSec() > 0 && (twist.linear.x != 0 || twist.linear.y != 0 || twist.angular.z != 0 || wasWalking > 0)) { //sendStep flag is set and velocity is nonzero or is first time zero  --> calculate next step and send it

		//figure out the next swing leg position and the relativ step
		tf::Transform deltaCenterSwing;
		tf::Transform step;
		if (currentLegToMove == 0) { //right leg
			deltaCenterSwing.setOrigin(tf::Point(0.0, -DEF_LEG_DISTANCE/2, 0.0));
			deltaCenterSwing.setRotation(tf::createQuaternionFromYaw(0.0));
			desiredRightLeg = desiredCenterPoint * deltaCenterSwing;   //place swing leg next to the desired center point
			step = desiredLeftLeg.inverse() * desiredRightLeg;
		} else { //left leg
			deltaCenterSwing.setOrigin(tf::Point(0.0, DEF_LEG_DISTANCE/2, 0.0));
			deltaCenterSwing.setRotation(tf::createQuaternionFromYaw(0.0));
			desiredLeftLeg = desiredCenterPoint * deltaCenterSwing;   //place swing leg next to the desired center point
			step = desiredRightLeg.inverse() * desiredLeftLeg;
		}

		//check if the step is feasible
		double deltaX = step.getOrigin().x();
		double deltaY = step.getOrigin().y();
		double deltaG = tf::getYaw(step.getRotation());

		deltaX = sgn(deltaX) * fmin(fabs(deltaX), MAX_STEP_LENGTH);
		deltaY = sgn(deltaY) * fmax(fmin(fabs(deltaY), MAX_LEG_DISTANCE), MIN_LEG_DISTANCE);
		deltaG = sgn(deltaG) * fmin(fabs(deltaG), MAX_STEP_ANGLE);

		tf::Pose satStep (tf::createQuaternionFromYaw(deltaG), tf::Point(deltaX, deltaY, 0.0));
		if (currentLegToMove == 0) { //right leg
			desiredRightLeg = desiredLeftLeg * satStep;
			desiredCenterPoint = desiredRightLeg * deltaCenterSwing.inverse();
		} else { //left leg
			desiredLeftLeg = desiredRightLeg * satStep;
			desiredCenterPoint = desiredLeftLeg * deltaCenterSwing.inverse();
		}

		// set the data for the step service and call it
		srv.request.step.pose.position.x = satStep.getOrigin().getX();
		srv.request.step.pose.position.y = satStep.getOrigin().getY();
		srv.request.step.pose.position.z = satStep.getOrigin().getZ();
		srv.request.step.pose.orientation.x = satStep.getRotation().getX();
		srv.request.step.pose.orientation.y = satStep.getRotation().getY();
		srv.request.step.pose.orientation.z = satStep.getRotation().getZ();
		srv.request.step.pose.orientation.w = satStep.getRotation().getW();
		srv.request.step.leg = currentLegToMove;
		footstepInc_client.call(srv); //call service

		// update next leg to move
		if (currentLegToMove == 0) {
			currentLegToMove = 1;
		} else {
			currentLegToMove = 0;
		}

		// check if this was the last step and the robot should move his legs parallel
		if (wasWalking <= 0) wasWalking = 2; //we need to final steps
		if (twist.linear.x == 0 && twist.linear.y == 0 && twist.angular.z == 0) wasWalking--;

		sendStep = false; //wait till next step is asked by robot
	}

	visualize();

	lastCallbackTime = ros::Time::now();
}

void stepsLeftCallback(std_msgs::UInt8 stepsLeftInBuffer) {
	if(stepsLeftInBuffer.data == 0){ //keine Schritte mehr im Buffer
		//ros::Duration(0.8).sleep(); //warten bevor neuer Schritt gesendet wird
		sendStep = true;
		sendStepTime = ros::Time::now() + ros::Duration(0.8);
	}
}



int main(int argc, char **argv)
{
  /* init ROS */

	ros::init(argc, argv, "biped_teleop");

	ros::NodeHandle n;
	ros::NodeHandle pn("~");

	//client zum senden der Schritt an den serializer
	footstepInc_client = n.serviceClient<biped_robin_msgs::StepTarget3DService>("footstep3DInc_srv");
	//subscriber zum Einlesen der Anzahl der Schritte im Buffer
	ros::Subscriber stepsLeftInBuffer_sub = n.subscribe<std_msgs::UInt8>("stepsLeftInBuffer", 1, stepsLeftCallback);
	//subscriber zum Einlesen der geschwindigkeit vom biped_joy_node
	ros::Subscriber twist = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, twistCallback);

	goalVizPub = pn.advertise<geometry_msgs::PoseStamped>("desiredCenterPoint", 10);

	//reste visualization
	ros::Duration(0.5).sleep();
	visualize();

	ros::spin();

	return 0;
}


